/*************************************************************
 Copyright (C) 2022 Earth Rover Limited. All rights reserved.
*************************************************************/

/**
 * @file correct_localization.h
 * @brief Header for the Correct Localization Class
 * @author Xavier Ruiz (xavier.ruiz@earthrover.farm)
 */

#ifndef __CORRECT_LOCALIZATION_H__
#define __CORRECT_LOCALIZATION_H__

#include <iostream>
#include <sstream>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <earth_rover_common_messages/LocalizationStatus.h>
#include <XmlRpcException.h>

class CorrectLocalization
{
    public:
         CorrectLocalization(const ros::NodeHandle &node_handle, const ros::NodeHandle &private_node_handle);
         ~CorrectLocalization() = default;

         std::vector<float> getArrayFromXmlValue(const XmlRpc::XmlRpcValue& param_in);
         void statusCb(const earth_rover_common_messages::LocalizationStatus& msg);
         void gpsCb(const sensor_msgs::NavSatFixConstPtr& msg);
         void headingCb(const sensor_msgs::ImuConstPtr& heading_msg);
         void ekfCb(const nav_msgs::OdometryConstPtr& ekf_msg);
         void logicCovariance(nav_msgs::Odometry& odom, const earth_rover_common_messages::LocalizationStatus& status);

     private:
         // Initialize Variables
         std::vector<float> m_covar_rtk_fix, m_covar_rtk_float, m_covar_dgnss, m_covar_sbas, m_covar_spp, m_covar_unknown;
         double m_gps_latency = 0.0;
         double m_ekf_latency = 0.0;
         earth_rover_common_messages::LocalizationStatus m_localization_status;

         // public and private ros node handle
         ros::NodeHandle m_nh;
         ros::NodeHandle m_pnh;

         // Subscribers and publishers
         ros::Subscriber m_gps_sub;
         ros::Subscriber m_heading_sub;
         ros::Subscriber m_ekf_sub;
         ros::Subscriber m_status_sub;
         ros::Publisher m_gps_pub;
         ros::Publisher m_heading_pub;
         ros::Publisher m_ekf_pub;
};

#endif
